﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

using Microsoft.Xna.Framework;
using Microsoft.Xna.Framework.Audio;
using Microsoft.Xna.Framework.Content;
using Microsoft.Xna.Framework.GamerServices;
using Microsoft.Xna.Framework.Graphics;
using Microsoft.Xna.Framework.Input;
using Microsoft.Xna.Framework.Media;

using Microsoft.Research.Kinect.Nui;

namespace KinnectInput
{
    public class SkeletonManager
    {
        public Runtime KinectSkelletonRuntime { get; private set; }

        //el esqueleto


        Vector3[] jointsPos;
        Vector2[] joints2DDisplay;
        GraphicsDevice gDev;
        Dictionary<JointID, Vector3> jointsPosition;
        public Dictionary<JointID, Vector2> jointsDisplayVe2 { get; private set; }
        Texture2D tex;
        public bool Debug = false;
        public Texture2D depth { get; private set; }
        public ImageResolution depthResolution;

        //constructor
        public SkeletonManager(GraphicsDevice gDev, ImageResolution depthResolution)
        {
            this.gDev = gDev;
            tex = new Texture2D(gDev, 1, 1);
            tex.SetData(new Color[] { Color.White });

            this.depthResolution = depthResolution;

            //Debo rellenar la texutra para que no reviente
            #region FillDeptTex
            Color[] cols;
            switch (depthResolution)
            {
                case ImageResolution.Resolution80x60:
                    cols = new Color[80 * 60];
                    depth = new Texture2D(this.gDev, 80, 60);
                    break;
                case ImageResolution.Resolution320x240:
                    cols = new Color[320 * 240];
                    depth = new Texture2D(this.gDev, 320, 240);
                    break;

                default:
                    cols = new Color[80 * 60];
                    depth = new Texture2D(this.gDev, 80, 60);
                    break;
            }
            for (int i = 0; i < cols.Length; i++)
            {
                cols[i] = Color.CornflowerBlue;
            }
            depth.SetData(cols);
            #endregion FillDeptTex

            joints2DDisplay = new Vector2[0];

            //instancio kinect
            KinectSkelletonRuntime = new Runtime();

            //inicializo
            KinectSkelletonRuntime.Initialize(RuntimeOptions.UseSkeletalTracking | RuntimeOptions.UseDepth);

            //me engancho al evento de skellteon y depts
            KinectSkelletonRuntime.SkeletonFrameReady += new EventHandler<SkeletonFrameReadyEventArgs>(KinectSkelletonRuntime_FrameReady);
            KinectSkelletonRuntime.DepthFrameReady += new EventHandler<ImageFrameReadyEventArgs>(KinectDepthRuntime_FrameReady);

            //falta listar joints en 3d i en 2d
            jointsPosition = new Dictionary<JointID, Vector3>();
            jointsDisplayVe2 = new Dictionary<JointID, Vector2>();
            //jointsPosition.Keys=KinectSkelletonRuntime.

            //abrimos stream de video
            KinectSkelletonRuntime.DepthStream.Open(ImageStreamType.Depth, 4, depthResolution, ImageType.Depth);

            //relleno el diccionario, con indice joint id que es un enum
            #region RellenarDiccionario
            jointsPosition.Add(JointID.Head, Vector3.Zero);
            jointsPosition.Add(JointID.HipCenter, Vector3.Zero);
            jointsPosition.Add(JointID.WristLeft, Vector3.Zero);
            jointsPosition.Add(JointID.WristRight, Vector3.Zero);
            jointsPosition.Add(JointID.HandLeft, Vector3.Zero);
            jointsPosition.Add(JointID.HandRight, Vector3.Zero);

            foreach (JointID ji in jointsPosition.Keys)
            {
                jointsDisplayVe2.Add(ji, Vector2.Zero);
            }
            #endregion RellenarDiccionario

        }

        //void runtime_SkeletonFrameReady(object sender, SkeletonFrameReadyEventArgs e)
        void KinectSkelletonRuntime_FrameReady(object sender, SkeletonFrameReadyEventArgs e)
        {

            SkeletonFrame skeletonFrame = e.SkeletonFrame;

            foreach (SkeletonData data in skeletonFrame.Skeletons)
            {
                if (SkeletonTrackingState.Tracked == data.TrackingState)
                {
                    joints2DDisplay = new Vector2[data.Joints.Count];
                    jointsPos = new Vector3[data.Joints.Count];
                    int i = 0;

                    // Joint data
                    foreach (Joint joint in data.Joints)
                    {
                        joints2DDisplay[i] = getDisplayPosition(joint);
                        jointsPos[i] = new Vector3(joint.Position.X, joint.Position.Y, joint.Position.Z);
                        i++;
                        JointID[] jointsIDs = jointsPosition.Keys.ToArray();
                        foreach (JointID j in jointsIDs)
                        {
                            if (joint.ID == j)
                            {
                                jointsPosition[joint.ID] = new Vector3(joint.Position.X, joint.Position.Y, joint.Position.Z);
                                jointsDisplayVe2[joint.ID] = getDisplayPosition(joint);
                            }
                        }
                    }
                }
            }

        }
        private Vector2 getDisplayPosition(Joint joint)
        {

            float depthX, depthY;
            KinectSkelletonRuntime.SkeletonEngine.SkeletonToDepthImage(joint.Position, out depthX, out depthY);
            //depthX = Math.Max(0, Math.Min(depthX * 320, 320));  //convert to 320, 240 space
            depthX = Math.Max(0, Math.Min(depthX * 320, gDev.Viewport.Width));//pasar  de  320 a screen space
            //depthY = Math.Max(0, Math.Min(depthY * 240, 240));  //convert to 320, 240 space
            depthY = Math.Max(0, Math.Min(depthY * 240, gDev.Viewport.Height));//de 320 ascreen space


            int colorX, colorY;
            ImageViewArea iv = new ImageViewArea();

            // only ImageResolution.Resolution640x480 is supported at this point
            KinectSkelletonRuntime.NuiCamera.GetColorPixelCoordinatesFromDepthPixel(ImageResolution.Resolution640x480, iv, (int)depthX, (int)depthY, (short)0, out colorX, out colorY);

            // map back to skeleton.Width & skeleton.Height
            //return new Vector2((int)(400 * colorX / 640.0), (int)(400 * colorY / 480));
            return new Vector2((int)(this.gDev.Viewport.Width * colorX / 640.0), (int)(this.gDev.Viewport.Height * colorY / 480));

        }
        void KinectDepthRuntime_FrameReady(object sender, ImageFrameReadyEventArgs e)
        {
            //depth = new Texture2D(graphics.GraphicsDevice, e.ImageFrame.Image.Width * e.ImageFrame.Image.BytesPerPixel, e.ImageFrame.Image.Height * e.ImageFrame.Image.BytesPerPixel);
            depth = new Texture2D(gDev, e.ImageFrame.Image.Width, e.ImageFrame.Image.Height);
            //le pido los bits a la imagen??
            byte[] ToDepthImageDisplay = e.ImageFrame.Image.Bits;

            //depth.SetData<byte>(e.ImageFrame.Image.Bits);
            //depth.SetData(ByteToColor(ToDepthImageDisplay));
            depth = Texture2DMirror.MirrorTex(ByteToColor(ToDepthImageDisplay), depth.Width, depth.Height, gDev);
            //isDepthReady = true;
        }

        private Color[] ByteToColor(byte[] ToDepthImageDisplay)
        {
            List<Color> colors = new List<Color>();
            for (int i = 0; i < ToDepthImageDisplay.Length; i += 2)
            {
                //gazapo copiado pero funciona transformar a imagen 16 bits
                int realDepth = (ToDepthImageDisplay[i + 1] << 5) | (ToDepthImageDisplay[i] >> 3);
                colors.Add(new Color(realDepth, realDepth, realDepth));

            }
            return colors.ToArray();
        }

        public void Draw(SpriteBatch sb)
        {
            sb.Draw(depth, gDev.Viewport.Bounds, Color.White);
            foreach (Vector2 flatVec in joints2DDisplay)
            {
                sb.Draw(tex, new Rectangle((int)flatVec.X - 10, (int)flatVec.Y - 10, 20, 20), Color.Red);
            }
            foreach (JointID ji in jointsDisplayVe2.Keys)
            {
                Vector2 flatVec = jointsDisplayVe2[ji];
                sb.Draw(tex, new Rectangle((int)flatVec.X - 10, (int)flatVec.Y - 10, 20, 20), Color.Blue);
            }
        }
    }
}
